#include <math.h>
#include "definicoes.h"
#include "fisica.h"
#include "stddef.h" //uso o NULL
#include "emulador.h"
#include "stdio.h"

//#define DEBUG 1

int caixa_compx=100;
int caixa_compy=100;

/*
int main(int argc, char** argv) {

}*/
void setTamanhoCaixa(int larguraCaixa, int alturaCaixa) {
	caixa_compx = larguraCaixa;
	caixa_compy = alturaCaixa;
}
//calcula novos componentes de vetores
void calcularComponentes(double vx,double vy, double angle, double *vxout,double *vyout ) {
	double modulo =sqrt(vx*vx + vy*vy)*0.87; // raiz (x^2+y^2)
	double radiano = (angle*M_PI)/180;
	double x,y;
	x = modulo * cos(radiano);
	y = modulo * sin(radiano);
	*vxout = x;
	*vyout = y;
}
void emulador_criarBola(int posInicialX,int posInicialY, int velocidadeImpulso, int angulo, TBola bolas[], int indexBola, double massa, double raio) {
	TBola *pbola = &bolas[indexBola];
	bolas[indexBola].raio = raio;
	bolas[indexBola].massa=massa;

	pbola->s0x=0;
	pbola->s0y=0;
	pbola->sx=posInicialX+raio;
	pbola->sy=posInicialY+raio;

	calcularComponentes(0, velocidadeImpulso,angulo,&pbola->vx, &pbola->vy);

}
void debug (TBola *pbola) {
	int loop=0;
	double tsolo=0;

	while (loop<100) {
		play(pbola);
		loop++;
	}

}
void calcularNovasVelocidadesY(double vx, double vy, double *vxout, double *vyout) {

	//a função atan2(vy,vx) retorna o ângulo e radiano, aí eu multiplico por 180 / Pi para obter o ângulo.
	double angulo = ( atan2(vy,vx) *180)/M_PI;
	calcularComponentes(vx,vy,angulo,vxout,vyout);
}

void printbola(TBola *pbola) {
	printf("[s0x:%4.2fm,s0y:%4.2fm sx:%4.2fm,sy:%4.2fm),v0x:%4.2fm/s,v0y:%4.2fm/s,vx:%4.2fm/s,vy:%4.2fm/s] \n",pbola->s0x,pbola->s0y,pbola->sx,pbola->sy,pbola->v0x,pbola->v0y,pbola->vx,pbola->vy);
}
void play(TBola *pbola) {
	double tlin2,tlin1;
	double s0x, s0y, v0y, v0x;

	s0x = pbola->sx;
	s0y = pbola->sy;
	v0x = pbola->vx;
	v0y = pbola->vy;

	pbola->s0x = s0x;
	pbola->s0y = s0y;
	pbola->v0x = v0x;
	pbola->v0y = v0y;

#ifdef DEBUG

	printbola(pbola);
#endif

	pbola->sx = MU_fhGetPosicao(s0x,v0x,TEMPO_AMOSTRAGEM);
	pbola->sy = MRUV_fhGetPosicao(s0y,v0y,TEMPO_AMOSTRAGEM,ACELERACAO);
	pbola->vy = MRUV_vtGetVelocidade(v0y,ACELERACAO,TEMPO_AMOSTRAGEM);

	if (pbola->sx > (caixa_compx-pbola->raio)) {
#ifdef DEBUG
		printf("\nCOLISÃO LADO DIREIT0: ");
		printbola(pbola);
#endif
		tlin1 = MU_fhGetTempo(s0x,caixa_compx-pbola->raio,v0x);		//tempo que o disco levou entre a amostragem anterior e a parede
		tlin2 = TEMPO_AMOSTRAGEM - tlin1; 							//parcela de tempo da amostragem que estará com a nova velocidade
		v0y = MRUV_vtGetVelocidade(v0y,ACELERACAO,tlin1);		//velocidade de colisão
		//pbola->vy = MRUV_vtGetVelocidade(v0y,-ACELERACAO,tlin2);	//oq é isso, será a nova velocidade?

		v0x = -v0x;
		s0y = MRUV_fhGetPosicao(s0y,v0y,tlin1,ACELERACAO);
		calcularNovasVelocidadesY(v0x,v0y,&v0x,&v0y);
		pbola->sx = MU_fhGetPosicao(caixa_compx-pbola->raio,v0x,tlin2);
		pbola->vx = v0x;

		pbola->sy = MRUV_fhGetPosicao(s0y,v0y,tlin2,ACELERACAO);
		v0y = MRUV_vtGetVelocidade(v0y,ACELERACAO,tlin2);
		pbola->vy = v0y;

	}else if(pbola->sx < pbola->raio) {
#ifdef DEBUG
		printf("\nCOLISÃO LADO ESQUERDO: ");
		printbola(pbola);
#endif
		tlin1 = MU_fhGetTempo(s0x,pbola->raio,v0x);
		tlin2 = TEMPO_AMOSTRAGEM - tlin1;
		v0y = MRUV_vtGetVelocidade(v0y,ACELERACAO,tlin1);		//velocidade de colisão

		v0x = -v0x;
		s0y = MRUV_fhGetPosicao(s0y,v0y,tlin1,ACELERACAO);
		calcularNovasVelocidadesY(v0x,v0y,&v0x,&v0y);
		pbola->sx = MU_fhGetPosicao(pbola->raio,v0x,tlin2);
		pbola->vx = v0x;
		pbola->sy = MRUV_fhGetPosicao(s0y,v0y,tlin2,ACELERACAO);
		v0y = MRUV_vtGetVelocidade(v0y,ACELERACAO,tlin2);
		pbola->vy = v0y;
	}
	if (pbola->sy > (caixa_compy-pbola->raio)) {
#ifdef DEBUG
		printf("\n COLISÃO TETO: ");
		printbola(pbola);
#endif
		pbola->vy = MRUV_torricelliGetVelocidade(v0y,ACELERACAO,caixa_compy - pbola->raio - s0y);
		tlin1 = MRUV_vtGetTempo(v0y,pbola->vy,ACELERACAO);
		tlin2 = TEMPO_AMOSTRAGEM - tlin1;
		s0x = MU_fhGetPosicao(s0x,v0x,tlin1);
		v0y = -pbola->vy;
		calcularNovasVelocidadesY(v0x,v0y,&v0x,&v0y);
		pbola->sx = MU_fhGetPosicao(s0x,v0x,tlin2);
		pbola->vx = v0x;
		pbola->sy = MRUV_fhGetPosicao(caixa_compy - pbola->raio,v0y,tlin2,-ACELERACAO);	//pq -aceleracao? se já é negativo
		pbola->vy = MRUV_vtGetVelocidade(v0y,ACELERACAO,tlin2);

	}else if (pbola->sy < pbola->raio) {
		//pbola->vy = -MRUV_torricelliGetVelocidade(v0y,ACELERACAO,s0y - pbola->raio);	//eu acho q a variacao do espaco esta ao contrario
		pbola->vy = -MRUV_torricelliGetVelocidade(v0y,ACELERACAO, pbola->raio -s0y);	//eu acho q a variacao do espaco esta ao contrario
#ifdef DEBUG
		printf("\nCOLISÃO PISO: ");
		printbola(pbola);
#endif

		tlin1 = MRUV_vtGetTempo(v0y,pbola->vy,ACELERACAO);
		tlin2 = TEMPO_AMOSTRAGEM - tlin1;
		s0x = MU_fhGetPosicao(s0x,v0x,tlin1);
		v0y = pbola->vy;
		calcularNovasVelocidadesY(v0x,v0y,&v0x,&v0y);
		pbola->sx = MU_fhGetPosicao(s0x,v0x,tlin2);
		pbola->vx = v0x;
		v0y = fabs(v0y);
		pbola->sy = MRUV_fhGetPosicao(pbola->raio,v0y,tlin2,ACELERACAO);
		pbola->vy = MRUV_vtGetVelocidade(v0y,ACELERACAO,tlin2);
		 if (pbola->sy < pbola->raio) {
			 pbola->sy = pbola->raio;
		 }
#ifdef DEBUG
		printf("\t");
		printbola(pbola);
#endif
	}
}

/*void verificarColisoes(TBola bolas[], int numBolas,int rank) {
	int i,j;
	double distanciaPontos, distanciaRaio;
	//outra forma de calcular é pela equação reduzida da circunferência será determinada por r^=(bsx-asx)^2 + (bsy-asy)^2
	//if ( (pow (bolas[j].sx - bolas[i].sx,2) + pow(bolas[j].sy - bolas[i].sy, 2)) <= pow(bolas[i].raio + bolas[j].raio, 2 ) )
	for (i=0;i<rank;i++)
		for (j= i+1;j<numBolas;j++)
			if (bolas[i].raio > 0 && bolas[j].raio > 0) {
				distanciaPontos = sqrt(pow (bolas[j].sx - bolas[i].sx,2) + pow(bolas[j].sy - bolas[i].sy, 2));
				distanciaRaio =bolas[i].raio+ bolas[j].raio;
				if (distanciaPontos <= distanciaRaio ) {
#ifdef DEBUG
		printf("\nDistanciaPontos:%4.2f, DistanciaRaio:%4.2f\n",distanciaPontos,distanciaRaio);
#endif
					colidirBolas(&bolas[i],&bolas[j]);
				}
			}
}*/

void colidirBolas (TBola *b1, TBola *b2) {
	double angulo_colisao,modulob1,modulob2,angulo_vel_b1,angulo_vel_b2,vx1,vx2,vy1,vy2,vx1a;

	if (b1->raio!=0 && b2->raio != 0) {
/*
		#ifdef DEBUG
		printf("\ANTES\n");
		printf("B1: ");
		//printbola(b1);
		printf("\tCOLISÃO [(s0x:%4.2fm,s0y:%4.2fm),v0x:%4.2fm/s,v0y:%4.2fm/s \n",b1->s0x,b1->s0y,b1->v0x,b1->v0y);
		printf("\tCOLISÃO [(sx:%4.2fm,sy:%4.2fm),vx:%4.2fm/s,vy:%4.2fm/s \n",b1->sx,b1->sy,b1->vx,b1->vy);
		printf("B2: ");
		//printbola(b2);
		printf("\tCOLISÃO [(s0x:%4.2fm,s0y:%4.2fm),v0x:%4.2fm/s,v0y:%4.2fm/s \n",b2->s0x,b2->s0y,b2->v0x,b2->v0y);
		printf("\tCOLISÃO [(sx:%4.2fm,sy:%4.2fm),vx:%4.2fm/s,vy:%4.2fm/s \n",b2->sx,b2->sy,b2->vx,b2->vy);
#endif*/
		//-------------------------------------------
/*
#ifdef DEBUG
		printf("\ANTES\n");
		printf("\nb1 v0x:%4.2f, v0y:%4.2f\n",b1->v0x,b1->v0y);
		printf("\nb2 v0x:%4.2f, v0y:%4.2f \n",b2->v0x,b2->v0y);
#endif*/
		angulo_colisao = atan2(b1->sy - b2->sy,b1->sx - b2->sx);		// TODO: estimado
		angulo_vel_b1 = atan2(b1->v0y ,b1->v0x);						// TODO: ASSUMINDO QUE A VELOCIDADE DE CHOQUE É IGUAL AO DA AMOSTRAGEM ANTERIOR PARA AMBAS AS BOLAS
		angulo_vel_b2 = atan2(b2->v0y ,b2->v0x);

/*#ifdef DEBUG
		printf("\nangulo_vel_b1x:%4.2f, angulo_vel_b2:%4.2f\n",angulo_vel_b1,angulo_vel_b2);
		printf("\nangulo de colisao:%4.2f\n",angulo_colisao);
#endif*/
				modulob1 = sqrt(b1->v0y*b1->v0y + b1->v0x*b1->v0x);
				modulob2 = sqrt(b2->v0y*b2->v0y + b2->v0x*b2->v0x);

				vx1 = modulob1*cos(angulo_vel_b1-angulo_colisao);
				vy1 = modulob1*sin(angulo_vel_b1-angulo_colisao);
				vx2 = modulob2*cos(angulo_vel_b2-angulo_colisao);
				vy2 = modulob2*sin(angulo_vel_b2-angulo_colisao);

				vx1a = ((b1->massa - b2->massa)*vx1 + (b1->massa + b2->massa)*vx2)/(b1->massa + b2->massa);
				vx2 = ((b2->massa - b1->massa)*vx2 + (b1->massa + b2->massa)*vx1)/(b1->massa + b2->massa);
				vx1 = vx1a;

				b1->v0x = vx1*cos(angulo_colisao) + vy1*cos(angulo_colisao + M_PI/2);
				b1->v0y = vx1*sin(angulo_colisao) + vy1*sin(angulo_colisao + M_PI/2);
				b2->v0x = vx2*cos(angulo_colisao) + vy2*cos(angulo_colisao + M_PI/2);
				b2->v0y = vx2*sin(angulo_colisao) + vy2*sin(angulo_colisao + M_PI/2);
/*#ifdef DEBUG
		printf("\nb1 v0x:%4.2f, v0y:%4.2f\n",b1->v0x,b1->v0y);
		printf("\nb2 v0x:%4.2f, v0y:%4.2f \n",b2->v0x,b2->v0y);
#endif*/
				modulob1 = sqrt(b1->v0y*b1->v0y + b1->v0x*b1->v0x);
				modulob2 = sqrt(b2->v0y*b2->v0y + b2->v0x*b2->v0x);
				angulo_vel_b1 = atan2(b1->v0y ,b1->v0x);
				angulo_vel_b2 = atan2(b2->v0y ,b2->v0x);
				modulob1 = COEFICIENTE_ELASTICIDADE * modulob1;
				modulob2 = COEFICIENTE_ELASTICIDADE * modulob2;

				b1->vx = modulob1*cos(angulo_vel_b1);
				b1->vy = modulob1*sin(angulo_vel_b1);
				b2->vx = modulob2*cos(angulo_vel_b2);
				b2->vy = modulob2*sin(angulo_vel_b2);

/*#ifdef DEBUG
				printf("\DEPOIS\n");
		printf("\nb1 v0x:%4.2f, v0y:%4.2f\n",b1->v0x,b1->v0y);
		printf("\nb2 v0x:%4.2f, v0y:%4.2f \n",b2->v0x,b2->v0y);
#endif*/

		b1->sx = MU_fhGetPosicao(b1->s0x, b1->vx, TEMPO_AMOSTRAGEM);		// TODO: NÃO É O TEMPO DE AMOSTRAGEM A ENTRADA DESTAS FUNÇÕES, É UM TEMPO MENOR QUE DEVERIA SER CALCULADO
		b2->sx = MU_fhGetPosicao(b2->s0x, b2->vx, TEMPO_AMOSTRAGEM);
		b1->sy = MRUV_fhGetPosicao(b1->s0y, b1->vy, TEMPO_AMOSTRAGEM, ACELERACAO);
		b2->sy = MRUV_fhGetPosicao(b2->s0y, b2->vy, TEMPO_AMOSTRAGEM, ACELERACAO);
		if (b2->sy < b2->raio) {
			b2->sy = b2->raio;
				 }
		if (b1->sy < b1->raio) {
			b1->sy = b1->raio;
						 }
		b1->v0y = b1->vy;
		b2->v0y = b2->vy;
	}
}
